// --------------------------------------------------------------------------------------------------------------------
// <copyright file="VelocityFromGamepad.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Runtime.Motion
{
    using System;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Navigation.Motion;
    using Microsoft.Robotics.Navigation.Runtime.Motion;
    using Microsoft.Robotics.Navigation.Runtime.Perception;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Implements random drive with gamepad override
    /// </summary>
    [DataContract]
    public class VelocityFromGamePad
    {
        /// <summary>
        /// Minimum speed, used for braking
        /// </summary>
        private const double MinSpeed = 0.001;

        /// <summary>
        /// Acceleration/deceleration factor, used to smooth changes in speed 
        /// </summary>
        private const double AccelerationFactor = 0.2;

        /// <summary>
        /// Speed to travel when moving forward
        /// </summary>
        private double maxSpeedInMetersPerSecond = default(double);

        /// <summary>
        /// Turning speed
        /// </summary>
        private double maxTurnRateInRadiansPerSecond = default(double);

        /// <summary>
        /// Maximum amount of time to turn for
        /// </summary>
        private double maximumTurnDurationInMs = default(double);

        /// <summary>
        /// Minimum amount of time to turn for
        /// </summary>
        private double minimumTurnDurationInMs = default(double);

        /// <summary>
        /// Current velocity with which we are turning in place
        /// </summary>
        private double currentTurnInPlaceVelocityInRadiansPerSecond;

        /// <summary>
        /// The current forward/backward velocity
        /// </summary>
        private double currentXVelocity;

        /// <summary>
        /// Target velocity with which we are turning in place
        /// </summary>
        private double targetTurnVelocity;

        /// <summary>
        /// Target current forward/backward velocity
        /// </summary>
        private double targetXVelocity;

        /// <summary>
        /// Initializes a new instance of the VelocityFromGamePad class
        /// </summary>
        /// <param name="maxSpeedInMetersPerSecond">Speed to travel when moving forward</param>
        /// <param name="maxTurnRateInRadiansPerSecond">Turning speed</param>
        /// <param name="maximumTurnDurationInMs">Maximum amount of time to turn for</param>
        /// <param name="minimumTurnDurationInMs">Minimum amount of time to turn for</param>
        public VelocityFromGamePad(
            double maxSpeedInMetersPerSecond, 
            double maxTurnRateInRadiansPerSecond, 
            double maximumTurnDurationInMs, 
            double minimumTurnDurationInMs)
        {
            this.maxSpeedInMetersPerSecond = maxSpeedInMetersPerSecond;
            this.maxTurnRateInRadiansPerSecond = maxTurnRateInRadiansPerSecond;
            this.maximumTurnDurationInMs = maximumTurnDurationInMs;
            this.minimumTurnDurationInMs = minimumTurnDurationInMs;
        }

        /// <summary>
        /// Handle MobileBaseVelocity messages
        /// </summary>
        /// <param name="message">Message with game pad state</param>
        /// <returns>A new MobileBaseVelocityReferenceMessage</returns>
        public MobileBaseVelocityReferenceMessage CreateMobileBaseVelocityReferenceMessage(GamePadAgentMessage message)
        {
            this.targetXVelocity = this.maxSpeedInMetersPerSecond * message.ThumbSticks.Left.Y;
            this.targetTurnVelocity = -this.maxTurnRateInRadiansPerSecond * message.ThumbSticks.Left.X;

            this.currentTurnInPlaceVelocityInRadiansPerSecond = this.currentTurnInPlaceVelocityInRadiansPerSecond
                                                                * (1 - AccelerationFactor)
                                                                + this.targetTurnVelocity * AccelerationFactor;

            this.currentXVelocity = this.currentXVelocity * (1 - AccelerationFactor) + this.targetXVelocity * AccelerationFactor;

            MobileBaseVelocityReference velocityUpdate = new MobileBaseVelocityReference(
                this.currentXVelocity, 0, this.currentTurnInPlaceVelocityInRadiansPerSecond);
            MobileBaseVelocityReferenceMessage newMessage = new MobileBaseVelocityReferenceMessage(velocityUpdate, message);
            return newMessage;
        }
    }
}
